//
// Created by engineai on 2024/07/03.
//
/*! @file SharedMemory.h
 *  @brief Shared memory utilities for connecting the simulator program to the
 * robot program
 *
 *
 */

#ifndef ZQ_HUMANOID_SHAREDMEMORY_H
#define ZQ_HUMANOID_SHAREDMEMORY_H

#include <fcntl.h>
#include <semaphore.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
#include <cassert>
#include <cstring>
#include <stdexcept>
#include <string>

#include "../cTypes.h"

#define DEVELOPMENT_SIMULATOR_SHARED_MEMORY_NAME "development-simulator"

/*!
 * A POSIX semaphore for shared memory.
 * See https://linux.die.net/man/7/sem_overview for more deatils
 */
class SharedMemorySemaphore
{
public:
    /*!
     * If semaphore is unitialized, initialize it and set its value.  This can be
     * called as many times as you want safely. It must be called at least once.
     * Only one process needs to call this, even if it is used in multiple
     * processes.
     *
     * Note that if init() is called after the semaphore has been initialized, it
     * will not change its value.
     * @param value The initial value of the semaphore.
     */
    void init(unsigned int value)
    {
        if (!_init)
        {
            if (sem_init(&_sem, 1, value))
            {
                printf("[ERROR] Failed to initialize shared memory semaphore: %s\n",
                       strerror(errno));
            }
            else
            {
                _init = true;
            }
        }
    }

    /*!
     * Increment the value of the semaphore.
     */
    void increment() { sem_post(&_sem); }

    /*!
     * If the semaphore's value is > 0, decrement the value.
     * Otherwise, wait until its value is > 0, then decrement.
     */
    void decrement() { sem_wait(&_sem); }

    /*!
     * If the semaphore's value is > 0, decrement the value and return true
     * Otherwise, return false (doesn't decrement or wait)
     * @return
     */
    bool tryDecrement() { return (sem_trywait(&_sem)) == 0; }

    /*!
     * Like decrement, but after waiting ms milliseconds, will give up
     * Returns true if the semaphore is successfully decremented
     */
    bool decrementTimeout(u64 seconds, u64 nanoseconds)
    {
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        ts.tv_nsec += nanoseconds;
        ts.tv_sec += seconds;
        ts.tv_sec += ts.tv_nsec / 1000000000;
        ts.tv_nsec %= 1000000000;
#ifdef linux
        return (sem_timedwait(&_sem, &ts) == 0);
#else
        return (sem_trywait(&_sem) == 0);
#endif
    }

    /*!
     * Delete the semaphore.  Note that deleting a semaphore in one process while
     * another is still using it results in very strange behavior.
     */
    void destroy() { sem_destroy(&_sem); }

private:
    sem_t _sem;
    bool _init = false;
};

/*!
 * A container class for an object which is stored in shared memory.  This
 * object can then be viewed in multiple processes or programs.  Note that there
 * is significant overhead when creating a shared memory object, so it is
 * recommended that two programs that communicate should have one single large
 * SharedMemoryObject instead of many small ones.
 *
 * A name string is used to identify shared objects across different programs
 *
 * Before a shared memory object can be used, you must either allocate new
 * memory, or connect it to an existing shared memory object.
 *
 * Creating/deleting the memory can be done with createNew/closeNew.
 * Viewing an existing object allocated with createNew can be done with
 * attach/detach
 *
 * For an example, see test_sharedMemory.cpp
 */
template <typename T>
class SharedMemoryObject
{
public:
    SharedMemoryObject() = default;

    /*!
     * Allocate memory for the shared memory object and attach to it.
     * If allowOverwrite is true, and there's already an object with this name,
     * the old object is overwritten Note that if this happens, the object may be
     * initialized in a very weird state.
     *
     * Otherwise, if an object with the name already exists, throws a
     * std::runtime_error
     */
    bool createNew(const std::string &name, bool allowOverwrite = false)
    {
        bool hadToDelete = false;
        assert(!_data);
        _name = name;
        _size = sizeof(T);
        printf("[Shared Memory] open new %s, size %ld bytes\n", name.c_str(),
               _size);

        _fd = shm_open(name.c_str(), O_RDWR | O_CREAT,
                       S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IROTH);
        if (_fd == -1)
        {
            printf("[ERROR] SharedMemoryObject shm_open failed: %s\n",
                   strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return false;
        }

        struct stat s;
        if (fstat(_fd, &s))
        {
            printf("[ERROR] SharedMemoryObject::createNew(%s) stat: %s\n",
                   name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return false;
        }

        if (s.st_size)
        {
            printf(
                "[Shared Memory] SharedMemoryObject::createNew(%s) on something that "
                "wasn't new (size is %ld bytes)\n",
                _name.c_str(), s.st_size);
            hadToDelete = true;
            if (!allowOverwrite)
                throw std::runtime_error(
                    "Failed to create shared memory - it already exists.");

            printf("\tusing existing shared memory!\n");
            // return false;
        }

        if (ftruncate(_fd, _size))
        {
            printf("[ERROR] SharedMemoryObject::createNew(%s) ftruncate(%ld): %s\n",
                   name.c_str(), _size, strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return false;
        }

        void *mem =
            mmap(nullptr, _size, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, 0);
        if (mem == MAP_FAILED)
        {
            printf("[ERROR] SharedMemory::createNew(%s) mmap fail: %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return false;
        }

        // there is a chance that the shared memory is not zeroed if we are reusing
        // old memory. this causes all sorts of weird issues, especially if the
        // layout of the object in memory has changed.
        memset(mem, 0, _size);

        _data = (T *)mem;
        return hadToDelete;
    }

    /*!
     * Attach to an existing shared memory object.
     */
    void attach(const std::string &name)
    {
        assert(!_data);
        _name = name;
        _size = sizeof(T);
        printf("[Shared Memory] open existing %s size %ld bytes\n", name.c_str(),
               _size);
        _fd = shm_open(name.c_str(), O_RDWR,
                       S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IROTH);
        if (_fd == -1)
        {
            printf("[ERROR] SharedMemoryObject::attach shm_open(%s) failed: %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        struct stat s;
        if (fstat(_fd, &s))
        {
            printf("[ERROR] SharedMemoryObject::attach(%s) stat: %s\n", name.c_str(),
                   strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        if ((size_t)s.st_size != _size)
        {
            printf(
                "[ERROR] SharedMemoryObject::attach(%s) on something that was "
                "incorrectly "
                "sized (size is %ld bytes, should be %ld)\n",
                _name.c_str(), s.st_size, _size);
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        void *mem =
            mmap(nullptr, _size, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, 0);
        if (mem == MAP_FAILED)
        {
            printf("[ERROR] SharedMemory::attach(%s) mmap fail: %s\n", _name.c_str(),
                   strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        _data = (T *)mem;
    }

    /*!
     * Free memory associated with the current open shared memory object.  The
     * object could have been opened with either attach or createNew.  After
     * calling this, no process can use this shared object
     */
    void closeNew()
    {
        assert(_data);
        // first, unmap
        if (munmap((void *)_data, _size))
        {
            printf("[ERROR] SharedMemoryObject::closeNew (%s) munmap %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        _data = nullptr;

        if (shm_unlink(_name.c_str()))
        {
            printf("[ERROR] SharedMemoryObject::closeNew (%s) shm_unlink %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        // close fd
        if (close(_fd))
        {
            printf("[ERROR] SharedMemoryObject::closeNew (%s) close %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        _fd = 0;
    }

    /*!
     * Close this view of the currently opened shared memory object. The object
     * can be opened with either attach or createNew.  After calling this, this
     * process can no longer use this shared object, but other processes still
     * can.
     */
    void detach()
    {
        assert(_data);
        // first, unmap
        if (munmap((void *)_data, _size))
        {
            printf("[ERROR] SharedMemoryObject::detach (%s) munmap %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        _data = nullptr;

        // close fd
        if (close(_fd))
        {
            printf("[ERROR] SharedMemoryObject::detach (%s) close %s\n",
                   _name.c_str(), strerror(errno));
            throw std::runtime_error("Failed to create shared memory!");
            return;
        }

        _fd = 0;
    }

    /*!
     * Get the shared memory object.
     */
    T *get()
    {
        assert(_data);
        return _data;
    }

    /*!
     * Get the shared memory object.
     */
    T &operator()()
    {
        assert(_data);
        return *_data;
    }

private:
    T *_data = nullptr;
    std::string _name;
    size_t _size;
    int _fd;
};
#endif // ZQ_HUMANOID_SHAREDMEMORY_H
